#include "slam_robot/odometry_publisher.hpp"
#include <tf2/LinearMath/Quaternion.h>
#include <cmath>

/**
 * @brief 构造函数
 * @param parent_node 父节点指针
 * @param serial SerialBridge共享指针
 */
OdometryPublisher::OdometryPublisher(rclcpp::Node* parent_node, 
                                    std::shared_ptr<SerialBridge> serial)
: parent_node_(parent_node), serial_(serial) {
    // 1. 创建里程计发布器
    odom_pub_ = parent_node_->create_publisher<nav_msgs::msg::Odometry>("/odom", 50);
    
    // 2. 创建TF广播器
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(parent_node_);
    
    // 3. 创建定时器（20ms周期）
    timer_ = parent_node_->create_wall_timer(
        std::chrono::milliseconds(20),
        std::bind(&OdometryPublisher::updateOdometry, this)
    );
    
    // 4. 初始化时间戳
    last_time_ = parent_node_->now();
    
    RCLCPP_INFO(parent_node_->get_logger(), "Odometry publisher initialized");
}

/**
 * @brief 更新里程计数据
 * 
 * 计算步骤：
 * 1. 获取当前速度数据
 * 2. 计算时间增量
 * 3. 积分计算新的位置和朝向
 * 4. 发布里程计消息和TF变换
 */
void OdometryPublisher::updateOdometry() {

    auto now = parent_node_->now();
    if (last_time_.nanoseconds() == 0) {
        last_time_ = now;
        return;
    }
    
    double dt = (now - last_time_).seconds();
    last_time_ = now;

    SpeedData data = serial_->getSpeedData();
    double vx = data.vx / 1000.0;
    double vy = data.vy / 1000.0;
    double wz = data.vz;
    
    // 修正坐标系转换
    double delta_x = (vx * cos(yaw_) - vy * sin(yaw_)) * dt;
    double delta_y = (vx * sin(yaw_) + vy * cos(yaw_)) * dt;
    
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        position_x_ += delta_x;
        position_y_ += delta_y;
        yaw_ += wz * dt;
        
        // 保存当前速度用于发布
        current_vx_ = vx;
        current_vy_ = vy;
        current_wz_ = wz;
    }
    
    publishOdometry(now);
    publishTF(now);
}

/**
 * @brief 发布里程计消息
 * @param now 当前时间戳
 */
void OdometryPublisher::publishOdometry(const rclcpp::Time& now) {

    double vx, vy, wz;
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        vx = current_vx_;
        vy = current_vy_;
        wz = current_wz_;
    }
    auto odom_msg = std::make_unique<nav_msgs::msg::Odometry>();

    SpeedData data = serial_->getSpeedData();
    
    // 填充消息头
    odom_msg->header.stamp = now;
    odom_msg->header.frame_id = "odom";
    odom_msg->child_frame_id = "base_link";
    
    // 设置位置
    odom_msg->pose.pose.position.x = position_x_;
    odom_msg->pose.pose.position.y = position_y_;
    odom_msg->pose.pose.position.z = 0.0;
    
    // 设置朝向
    tf2::Quaternion q;
    q.setRPY(0, 0, yaw_);
    odom_msg->pose.pose.orientation.x = q.x();
    odom_msg->pose.pose.orientation.y = q.y();
    odom_msg->pose.pose.orientation.z = q.z();
    odom_msg->pose.pose.orientation.w = q.w();
    
    // 设置速度 (在base_link坐标系中)
    odom_msg->twist.twist.linear.x = vx; 
    odom_msg->twist.twist.linear.y = vy;
    odom_msg->twist.twist.angular.z = wz;
    
    // 发布里程计
    odom_pub_->publish(std::move(odom_msg));
}

/**
 * @brief 发布TF变换
 * @param now 当前时间戳
 */
void OdometryPublisher::publishTF(const rclcpp::Time& now) {
    geometry_msgs::msg::TransformStamped transform;
    
    // 填充TF消息
    transform.header.stamp = now;
    transform.header.frame_id = "odom";
    transform.child_frame_id = "base_link";
    
    transform.transform.translation.x = position_x_;
    transform.transform.translation.y = position_y_;
    transform.transform.translation.z = 0.0;
    
    tf2::Quaternion q;
    q.setRPY(0, 0, yaw_);
    transform.transform.rotation.x = q.x();
    transform.transform.rotation.y = q.y();
    transform.transform.rotation.z = q.z();
    transform.transform.rotation.w = q.w();
    
    // 广播TF
    tf_broadcaster_->sendTransform(transform);
}